
#include <BSixdofRestraint>
#include "member_BSixdofRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

#define member					(*(member_BSixdofRestraint*)_ptr)
#define member_allocate()		_ptr = new member_BSixdofRestraint(this)
#define member_release()		delete (member_BSixdofRestraint*)_ptr

BSixdofRestraint::BSixdofRestraint()
{
	member_allocate();
}
BSixdofRestraint::BSixdofRestraint(const BString& name) : BRestraint(name)
{
	member_allocate();
}
BSixdofRestraint::BSixdofRestraint(BBody* bodyA, const BVector& pivotA) : BRestraint(bodyA, pivotA)
{
	member_allocate();
}
BSixdofRestraint::BSixdofRestraint(BBody* bodyA, BBody* bodyB) : BRestraint(bodyA, bodyB)
{
	member_allocate();
}
BSixdofRestraint::BSixdofRestraint(BBody* bodyA, const BVector& pivotA, BBody* bodyB, const BVector& pivotB) : BRestraint(bodyA, pivotA, bodyB, pivotB)
{
	member_allocate();
}
BSixdofRestraint::~BSixdofRestraint()
{
	member_release();
}

void BSixdofRestraint::copy(const BRestraint* other)
{
	BRestraint::copy(other);
}
BRestraint* BSixdofRestraint::clone() const
{
	BSixdofRestraint* restraint = new BSixdofRestraint();
	restraint->copy(this);
	return restraint;
}

void BSixdofRestraint::setLinearLowerLimit(BReal minx, BReal miny, BReal minz)
{
	member.linearLimits[0].range.min() = minx;
	member.linearLimits[1].range.min() = miny;
	member.linearLimits[2].range.min() = minz;
}
void BSixdofRestraint::setLinearLowerLimit(const BVector& linearLower)
{
	member.linearLimits[0].range.min() = linearLower.x();
	member.linearLimits[1].range.min() = linearLower.y();
	member.linearLimits[2].range.min() = linearLower.z();
}
BVector BSixdofRestraint::linearLowerLimit() const
{
	BReal x = member.linearLimits[0].range.min();
	BReal y = member.linearLimits[1].range.min();
	BReal z = member.linearLimits[2].range.min();
	return BVector(x, y, z);
}

void BSixdofRestraint::setLinearUpperLimit(BReal maxx, BReal maxy, BReal maxz)
{
	member.linearLimits[0].range.max() = maxx;
	member.linearLimits[1].range.max() = maxy;
	member.linearLimits[2].range.max() = maxz;
}
void BSixdofRestraint::setLinearUpperLimit(const BVector& linearUpper)
{
	member.linearLimits[0].range.max() = linearUpper.x();
	member.linearLimits[1].range.max() = linearUpper.y();
	member.linearLimits[2].range.max() = linearUpper.z();
}
BVector BSixdofRestraint::linearUpperLimit() const
{
	BReal x = member.linearLimits[0].range.max();
	BReal y = member.linearLimits[1].range.max();
	BReal z = member.linearLimits[2].range.max();
	return BVector(x, y, z);
}

void BSixdofRestraint::setAngularLowerLimit(BReal minx, BReal miny, BReal minz)
{
	member.angularLimits[0].range.min() = NormalizeAngle(minx);
	member.angularLimits[1].range.min() = NormalizeAngle(miny);
	member.angularLimits[2].range.min() = NormalizeAngle(minz);
}
void BSixdofRestraint::setAngularLowerLimit(const BVector& angularLower)
{
	member.angularLimits[0].range.min() = NormalizeAngle(angularLower.x());
	member.angularLimits[1].range.min() = NormalizeAngle(angularLower.y());
	member.angularLimits[2].range.min() = NormalizeAngle(angularLower.z());
}
BVector BSixdofRestraint::angularLowerLimit() const
{
	BReal x = member.angularLimits[0].range.min();
	BReal y = member.angularLimits[1].range.min();
	BReal z = member.angularLimits[2].range.min();
	return BVector(x, y, z);
}

void BSixdofRestraint::setAngularUpperLimit(BReal maxx, BReal maxy, BReal maxz)
{
	member.angularLimits[0].range.max() = NormalizeAngle(maxx);
	member.angularLimits[1].range.max() = NormalizeAngle(maxy);
	member.angularLimits[2].range.max() = NormalizeAngle(maxz);
}
void BSixdofRestraint::setAngularUpperLimit(const BVector& angularUpper)
{
	member.angularLimits[0].range.max() = NormalizeAngle(angularUpper.x());
	member.angularLimits[1].range.max() = NormalizeAngle(angularUpper.y());
	member.angularLimits[2].range.max() = NormalizeAngle(angularUpper.z());
}
BVector BSixdofRestraint::angularUpperLimit() const
{
	BReal x = member.angularLimits[0].range.max();
	BReal y = member.angularLimits[1].range.max();
	BReal z = member.angularLimits[2].range.max();
	return BVector(x, y, z);
}

void BSixdofRestraint::setLinearMotorEnabled(int index, bool motorEnabled)
{
	member.linearLimits[index].enabled = motorEnabled;
}
bool BSixdofRestraint::linearMotorEnabled(int index) const
{
	return member.linearLimits[index].enabled;
}

void BSixdofRestraint::setLinearMotorTarget(int index, BReal motorTarget)
{
	member.linearLimits[index].motorTarget = motorTarget;
}
BReal BSixdofRestraint::linearMotorTarget(int index) const
{
	return member.linearLimits[index].motorTarget;
}

void BSixdofRestraint::setLinearMotorVelocity(int index, BReal motorVelocity)
{
	member.linearLimits[index].motorVelocity = motorVelocity;
}
BReal BSixdofRestraint::linearMotorVelocity(int index) const
{
	return member.linearLimits[index].motorVelocity;
}

void BSixdofRestraint::setLinearMotorMaxForce(int index, BReal motorMaxForce) const
{
	member.linearLimits[index].motorMaxForce = motorMaxForce;
}
BReal BSixdofRestraint::linearMotorMaxForce(int index) const
{
	return member.linearLimits[index].motorMaxForce;
}

void BSixdofRestraint::setAngularMotorEnabled(int index, bool motorEnabled)
{
	member.angularLimits[index].enabled = motorEnabled;
}
bool BSixdofRestraint::angularMotorEnabled(int index) const
{
	return member.angularLimits[index].enabled;
}

void BSixdofRestraint::setAngularMotorTarget(int index, BReal motorTarget)
{
	member.angularLimits[index].motorTarget = motorTarget;
}
BReal BSixdofRestraint::angularMotorTarget(int index) const
{
	return member.angularLimits[index].motorTarget;
}

void BSixdofRestraint::setAngularMotorVelocity(int index, BReal motorVelocity)
{
	member.angularLimits[index].motorVelocity = motorVelocity;
}
BReal BSixdofRestraint::angularMotorVelocity(int index) const
{
	return member.angularLimits[index].motorVelocity;
}

void BSixdofRestraint::setAngularMotorMaxForce(int index, BReal motorMaxForce) const
{
	member.angularLimits[index].motorMaxForce = motorMaxForce;
}
BReal BSixdofRestraint::angularMotorMaxForce(int index) const
{
	return member.angularLimits[index].motorMaxForce;
}

void BSixdofRestraint::enableSpring(int index, bool onOff)
{
	member.springEnabled[index] = onOff;
	member.linearLimits[index].enabled = onOff;
}
bool BSixdofRestraint::isSpringEnabled(int index) const
{
	return member.springEnabled[index];
}

void BSixdofRestraint::setStiffness(int index, BReal stiffness)
{
	member.springStiffness[index] = stiffness;
}
BReal BSixdofRestraint::getStiffness(int index) const
{
	return member.springStiffness[index];
}

void BSixdofRestraint::setDamping(int index, BReal damping)
{
	member.springDamping[index] = damping;
}
BReal BSixdofRestraint::getDamping(int index) const
{
	return member.springDamping[index];
}

int BSixdofRestraint::solverSize()
{
	member.calculateTransforms();
	int num = 0;
	for (int i = 0; i < 3; i++)
	{
		LimitMotor& limitMotor = member.angularLimits[i];
		if (limitMotor.limited || limitMotor.enabled)
		{
			num++;
		}
	}
	for (int i = 0; i < 3; i++)
	{
		LimitMotor& limitMotor = member.linearLimits[i];
		if (limitMotor.limited || limitMotor.enabled)
		{
			num++;
		}
	}
	return num;
}
void BSixdofRestraint::soverItems(BSolverItem* solverItems, BReal stepTime)
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	for (int i = 0; i < 3; i++)
	{
		if (member.springEnabled[i])
		{
			LimitMotor& limitMotor = member.linearLimits[i];
			BReal currPos = limitMotor.currentValue;
			BReal delta = currPos - member.equilibriumPoint[i];
			BReal force = delta * member.springStiffness[i];
			BReal velFactor = member.springDamping[i] / stepTime;
			limitMotor.motorVelocity = velFactor * force;
			limitMotor.motorMaxForce = fabs(force);
		}
	}
	for (int i = 0; i < 3; i++)
	{
		if (member.springEnabled[i + 3])
		{
			LimitMotor& limitMotor = member.angularLimits[i];
			BReal currPos = limitMotor.currentValue;
			BReal delta = currPos - member.equilibriumPoint[i + 3];
			BReal force = -delta * member.springStiffness[i + 3];
			BReal velFactor = member.springDamping[i + 3] / stepTime;
			limitMotor.motorVelocity = velFactor * force;
			limitMotor.motorMaxForce = fabs(force);
		}
	}

	int row = 0;

	for (int i = 0; i < 3; i++)
	{
		LimitMotor& limitMotor = member.angularLimits[i];
		if (limitMotor.limited || limitMotor.enabled)
		{
			const BVector& axis = member.calculatedAxis[i];
			member.applyAngularLimit(stepTime, solverItems[row++], limitMotor, axis);
		}
	}

	for (int i = 0; i < 3; i++)
	{
		LimitMotor& limitMotor = member.linearLimits[i];
		if (limitMotor.limited || limitMotor.enabled)
		{
			BVector axis = matrixA.row(i);
			int indx1 = (i + 1) % 3;
			int indx2 = (i + 2) % 3;
			int rotAllowed = 1;
			if (member.angularLimits[indx1].limited && member.angularLimits[indx2].limited)
			{
				rotAllowed = 0;
			}
			member.applyLinearLimit(stepTime, solverItems[row++], limitMotor, axis, rotAllowed);
		}
	}
}

BVector BSixdofRestraint::offset() const
{
	BReal x = member.linearLimits[0].currentValue;
	BReal y = member.linearLimits[1].currentValue;
	BReal z = member.linearLimits[2].currentValue;
	return BVector(x, y, z);
}
BVector BSixdofRestraint::angle() const
{
	BReal rx = member.angularLimits[0].currentValue;
	BReal ry = member.angularLimits[1].currentValue;
	BReal rz = member.angularLimits[2].currentValue;
	return BVector(rx, ry, rz);
}
